/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "stdafx.h"
#include "MotionPlanning_Kine.h"

namespace Robotic { namespace Mechanism{
	using namespace std;
	using namespace Robotic::Math;

	MotionPlanning_Kine::MotionPlanning_Kine(Chain* pRobot, double stepInterval):MotionPlanning(pRobot, stepInterval)
	{
		_motionSegments = std::vector<MotionSegment_Kine>();
		_currentSegmentPos = 0;
		_currentSegmentVel = 0;
		_currentSegmentAcc = 0;

		_isStop = false;

		_Kd = 1;
		_Kp = 1;
	}

	MotionPlanning_Kine::~MotionPlanning_Kine()
	{
		_motionSegments.clear();
	}

	double MotionPlanning_Kine::Get_Kd()
	{
		//ColumnVector Kd = ColumnVector(_pRobot->Get_Dof());
		//for(int i=0; i<_pRobot->Get_Dof(); i++)
		//	Kd[i] = _Kd(i,i);
		return _Kd;
	}

	double MotionPlanning_Kine::Get_Kp()
	{
		//ColumnVector Kp = ColumnVector(_pRobot->Get_Dof());
		//for(int i=0; i<_pRobot->Get_Dof(); i++)
		//	Kp[i] = _Kp(i,i);
		return _Kp;
	}

	void MotionPlanning_Kine::Set_Kd(const double & Kd)
	{
		//if(Kd.GetCount() != _pRobot->Get_Dof())
		//{
		//	throw(new exception(" Argument Kd's count number should be the same as _dof."));
		//}

		//_Kd.SetZero();
		//for(int i=0; i<_pRobot->Get_Dof(); i++)
		//	_Kd(i,i) = Kd[i];
		_Kd = Kd;
	}

	void MotionPlanning_Kine::Set_Kp(const double & Kp)
	{
		//if(Kp.GetCount() != _pRobot->Get_Dof())
		//{
		//	throw(new exception(" Argument Kp's count number should be the same as _dof."));
		//}

		//_Kp.SetZero();
		//for(int i=0; i<_pRobot->Get_Dof(); i++)
		//	_Kp(i,i) = Kp[i];
		_Kp = Kp;
	}

	void MotionPlanning_Kine::MoveForward(Velocity vel, bool global)
	{
		_isStop = false;
	}

	void MotionPlanning_Kine::MoveJoint(const ColumnVector toJointValues, double speed, double flyByZone)
	{
		_isStop = false;

		if(flyByZone < _minFlyByZone)
			flyByZone = 0;

		//get current robot pose
		ColumnVector startJoints;//jiont value
		Frame3 startFrame;//robot tool frame pose w.r.t global frame
		if(_motionSegments.size() > 0)
		{
			startJoints = _motionSegments.back().TargetJoints;
			startFrame = _motionSegments.back().TargetFrame;
		}
		else
		{
			startJoints = _pRobot->Get_JointValues();
			Frame3 curEEPose = _pRobot->ForwardKine_Pose(startJoints);
			startFrame = _robotBaseFrame * curEEPose * _toolFrame;
		}

		//get max change joint value
		double maxChange = abs(toJointValues[0] - startJoints[0]);
		for(int i=1; i< toJointValues.GetCount(); i++)
		{
			double change = abs(toJointValues[i] - startJoints[i]);
			if(maxChange < change)
				maxChange = change;
		}


		//create motion segment
		MotionSegment_Kine motionSegment;
		motionSegment.IsZone = false;
		motionSegment.MotionSegmentType = MotionSegment_Kine_Joint;
		motionSegment.StartJoints = startJoints;
		motionSegment.StartFrame = startFrame;
		motionSegment.TargetJoints = toJointValues;
		motionSegment.TargetFrame = _robotBaseFrame * _pRobot->ForwardKine_Pose(toJointValues) * _toolFrame;
		motionSegment.CenterPoint = Vector3::Zero();
		motionSegment.ChangeLength = maxChange;
		motionSegment.ChangeOrient = Quaternion(motionSegment.TargetFrame.Rotation * motionSegment.StartFrame.Rotation.Inverse());
		motionSegment.ToolFrame = _toolFrame;
		motionSegment.Speed = speed;
		motionSegment.FlyByZone = flyByZone;
		_motionSegments.push_back(motionSegment);
	}

	void MotionPlanning_Kine::MoveCart(const Frame3 toPoint, double speed, double flyByZone)
	{
		_isStop = false;

		if(flyByZone < _minFlyByZone)
			flyByZone = 0;

		//get previous start robot pose
		Frame3 prevTargetFrame;//robot tool frame pose w.r.t global frame
		ColumnVector prevTargetJoint;//robot joint values
		if(_motionSegments.size() > 0)
		{
			prevTargetJoint = _motionSegments.back().TargetJoints;
			prevTargetFrame = _motionSegments.back().TargetFrame;
		}
		else
		{
			prevTargetJoint = _pRobot->Get_JointValues();
			prevTargetFrame = _robotBaseFrame * _pRobot->ForwardKine_Pose(prevTargetJoint) * _toolFrame;
		}

		//get target frame and target joint values
		Frame3 targetFrame = toPoint * _objectFrame;//robot target frame w.r.t global frame
		ColumnVector targetJoint;
		Frame3 endEffecter = _robotBaseFrame.Inverse() * targetFrame * _toolFrame.Inverse();
		if(GetJointValues(endEffecter, targetJoint) == false)
			throw(new exception(" Argument toPoint is unreachable."));


		//get max change joint value
		double maxChange = abs(targetJoint[0] - prevTargetJoint[0]);
		for(int i=1; i< targetJoint.GetCount(); i++)
		{
			double change = abs(targetJoint[i] - prevTargetJoint[i]);
			if(maxChange < change)
				maxChange = change;
		}

		//create motion segment
		MotionSegment_Kine motionSegment;
		motionSegment.IsZone = false;
		motionSegment.MotionSegmentType = MotionSegment_Kine_Joint;
		motionSegment.StartJoints = prevTargetJoint;
		motionSegment.StartFrame = prevTargetFrame;
		motionSegment.TargetJoints = targetJoint;
		motionSegment.TargetFrame = targetFrame;
		motionSegment.CenterPoint = Vector3::Zero();
		motionSegment.ChangeLength = maxChange;
		motionSegment.ChangeOrient = Quaternion(motionSegment.TargetFrame.Rotation * motionSegment.StartFrame.Rotation.Inverse());
		motionSegment.ToolFrame = _toolFrame;
		motionSegment.Speed = speed;
		motionSegment.FlyByZone = flyByZone;
		_motionSegments.push_back(motionSegment);

	}

	void MotionPlanning_Kine::MoveCart_Linear(const Frame3 toPoint, double speed, double flyByZone)
	{
		_isStop = false;

		if(flyByZone < _minFlyByZone)
			flyByZone = 0;

		//get previous start robot pose
		Frame3 prevTargetFrame;//robot tool frame pose w.r.t global frame
		ColumnVector prevTargetJoint;//robot joint values
		if(_motionSegments.size() > 0)
		{
			prevTargetJoint = _motionSegments.back().TargetJoints;
			prevTargetFrame = _motionSegments.back().TargetFrame;
		}
		else
		{
			prevTargetJoint = _pRobot->Get_JointValues();
			prevTargetFrame = _robotBaseFrame * _pRobot->ForwardKine_Pose(prevTargetJoint) * _toolFrame;
		}

		//get target frame and target joint values
		Frame3 targetFrame = toPoint * _objectFrame;//robot target frame w.r.t global frame
		ColumnVector targetJoint;
		Frame3 endEffecter = _robotBaseFrame.Inverse() * targetFrame * _toolFrame.Inverse();
		if(GetJointValues(endEffecter, targetJoint) == false)
			throw(new exception(" Argument toPoint is unreachable."));

		//if previous segment is zone and is not executing segment, and this segment is out of previous segment's range,
		//modify it to make it connect previous and this segment.
		Vector3 direction = targetFrame.Translation - prevTargetFrame.Translation;
		double dis = direction.Norm();
		if(dis <= 0)
		{
			direction[0] = 0;
			direction[1] = 0;
			direction[2] = 1;
		}
		else
			direction.Normalize();
		if(_motionSegments.size() >= 2)
		{
			MotionSegment_Kine &lastSegment = _motionSegments.back();
			if((lastSegment.IsZone == true) && (lastSegment.FlyByZone < dis))
			{
				//calculate Center point
				//        M
				//        /\
				//       /  \
				//     S/    \E
				//        .Center point
				//
				Vector3 S = lastSegment.StartFrame.Translation;
				Vector3 M = lastSegment.TargetFrame.Translation;
				Vector3 E = M + direction * lastSegment.FlyByZone;
				Vector3 SE_Mid = (S+E)/2;
				double theta = acos( 1 - sqr((E-S).Norm())/(2*(S-M).Norm()*(E-M).Norm()) );//0 ~ PI
				double cosHalfTheta = cos(theta/2);
				Vector3 centerPoint = M + (SE_Mid-M)/(cosHalfTheta * cosHalfTheta); 
				double radius = (S - centerPoint).Norm();
				
				//get target robot pose
				Frame3 targetFrame2 = Frame3(lastSegment.TargetFrame.Rotation,E);//robot target frame w.r.t global frame
				ColumnVector targetJoint2;
				Frame3 endEffecter2 = _robotBaseFrame.Inverse() * targetFrame2 * lastSegment.ToolFrame.Inverse();
				if(GetJointValues(endEffecter2, targetJoint2) == false)
					throw(new exception(" Argument toPoint is unreachable."));

				//modify last segment
				lastSegment.MotionSegmentType = MotionSegment_Kine_CartCycle;
				lastSegment.TargetJoints = targetJoint2;
				lastSegment.TargetFrame = targetFrame2;
				lastSegment.CenterPoint = centerPoint;
				lastSegment.ChangeLength = radius * (PI- theta);
				lastSegment.ChangeOrient = Quaternion(lastSegment.TargetFrame.Rotation * lastSegment.StartFrame.Rotation.Inverse());

				prevTargetJoint = lastSegment.TargetJoints;
				prevTargetFrame = lastSegment.TargetFrame;
				dis = dis - lastSegment.FlyByZone;
			}
		}

		//create this segment
		if(dis > (flyByZone))
		{
			//get target robot pose
			Frame3 targetFrame3 = Frame3(targetFrame.Rotation, targetFrame.Translation - direction * flyByZone);//robot target frame w.r.t global frame
			ColumnVector targetJoint3;
			Frame3 endEffecter3 = _robotBaseFrame.Inverse() * targetFrame3 * _toolFrame.Inverse();
			if(GetJointValues(endEffecter3, targetJoint3) == false)
				throw(new exception(" Argument toPoint is unreachable."));


			MotionSegment_Kine motionSegment;
			motionSegment.IsZone = false;
			motionSegment.MotionSegmentType = MotionSegment_Kine_CartLinear;
			motionSegment.StartJoints = prevTargetJoint;
			motionSegment.StartFrame = prevTargetFrame;
			motionSegment.TargetJoints = targetJoint3;
			motionSegment.TargetFrame = targetFrame3;
			motionSegment.CenterPoint = Vector3::Zero();
			motionSegment.ChangeLength = (motionSegment.TargetFrame.Translation - motionSegment.StartFrame.Translation).Norm();
			motionSegment.ChangeOrient = Quaternion(motionSegment.TargetFrame.Rotation * motionSegment.StartFrame.Rotation.Inverse());
			motionSegment.ToolFrame = _toolFrame;
			motionSegment.Speed = speed;
			motionSegment.FlyByZone = flyByZone;
			_motionSegments.push_back(motionSegment);

			prevTargetJoint = targetJoint3;
			prevTargetFrame = targetFrame3;
		}

		//create zone segment after this segment
		MotionSegment_Kine motionSegment;
		motionSegment.IsZone = true;
		motionSegment.MotionSegmentType = MotionSegment_Kine_CartLinear;
		motionSegment.StartJoints = prevTargetJoint;
		motionSegment.StartFrame = prevTargetFrame;
		motionSegment.TargetJoints = targetJoint;
		motionSegment.TargetFrame = targetFrame;
		motionSegment.CenterPoint = Vector3::Zero();
		motionSegment.ChangeLength = (motionSegment.TargetFrame.Translation - motionSegment.StartFrame.Translation).Norm();
		motionSegment.ChangeOrient = Quaternion(motionSegment.TargetFrame.Rotation * motionSegment.StartFrame.Rotation.Inverse());
		motionSegment.ToolFrame = _toolFrame;
		motionSegment.Speed = speed;
		motionSegment.FlyByZone = flyByZone;
		_motionSegments.push_back(motionSegment);
	}

	void MotionPlanning_Kine::Stop(const bool followPath)
	{
		_isStop = true;
	}

	int MotionPlanning_Kine::NextStep()
	{
		//pause motion if ordered.
		if(_isStop == true)
		{
			//_currentSegmentPos = newSegmentPos;//stop at current postion
			_currentSegmentVel = 0;
			_currentSegmentAcc = 0;

			//set values
			//_pRobot->Set_JointValues(newJointValue);
			ColumnVector temp(_pRobot->Get_Dof());
			temp.SetZero();
			_pRobot->Set_JointVels(temp);
			_pRobot->Set_JointAccs(temp);
		}

		//remove too short segment
		while(_motionSegments.size() > 0)
		{
			if(Equal(_motionSegments.front().ChangeLength, 0))
				_motionSegments.erase(_motionSegments.begin());
			else
				break;
		}

		if(_motionSegments.size() <= 0)
			return -1;

		//get current executing segment
		MotionSegment_Kine motionSegment = _motionSegments[0];

		////calculate next step
		//ColumnVector q = _pRobot->Get_JointValues();//jiont value
		//ColumnVector qp = _pRobot->Get_JointVels();//jiont velocity

		//Frame3 curEEPose = _pRobot->ForwardKine_Pose(q);//robot end effector pose w.r.t robot base
		//Velocity curEEVel = _pRobot->ForwardKine_Vel(q, qp);//robot end effector velocity w.r.t robot base

		//Frame3 curToolPose = _robotBaseFrame * curEEPose * motionSegment.ToolFrame;//robot tool frame pose w.r.t global frame
		//Velocity curToolVel = curEEVel.RefPoint(motionSegment.ToolFrame.Translation).RefBaseFrame(_robotBaseFrame);//robot tool frame velocity w.r.t global frame

		//double curToolSpeed = curToolVel.LinearVel.Norm();//robot tool frame's motion speed along trajectory

		if( motionSegment.MotionSegmentType == MotionSegment_Kine_Joint)
		{
			//get new pose, vel, acc along segment trajectory
			double newSegmentAcc = Cmd_Acc(_currentSegmentPos, _currentSegmentVel, motionSegment.ChangeLength, (_motionSegments.size()==1)?0:motionSegment.Speed);
			if(newSegmentAcc > 0)
			{
				//don't accelerate when is on Speed limit.
				if(_currentSegmentVel >= motionSegment.Speed)
					newSegmentAcc = 0;
				else
				{
					//don't make next speed out of speed limit
					double maxAcc = (motionSegment.Speed - _currentSegmentVel)/_stepInterval;
					if(newSegmentAcc > maxAcc)
						newSegmentAcc = maxAcc;
				}
			}

			double newSegmentVel = _currentSegmentVel + newSegmentAcc * _stepInterval;
			double newSegmentPos = _currentSegmentPos + _currentSegmentVel * _stepInterval + 0.5 * newSegmentAcc * _stepInterval * _stepInterval;
			if(newSegmentPos > motionSegment.ChangeLength)
			{
				newSegmentPos = motionSegment.ChangeLength;
				newSegmentVel = (newSegmentPos - _currentSegmentPos)*2/_stepInterval - _currentSegmentVel;
				newSegmentAcc = (newSegmentVel - _currentSegmentVel)/_stepInterval;
			}
			_currentSegmentPos = newSegmentPos;
			_currentSegmentVel = newSegmentVel;
			_currentSegmentAcc = newSegmentAcc;

			//set joint value, vel and acc.
			ColumnVector newJointValue(_pRobot->Get_Dof());
			ColumnVector newJointVel(_pRobot->Get_Dof());
			ColumnVector newJointAcc(_pRobot->Get_Dof());
			for(int i=0; i<_pRobot->Get_Dof(); i++)
			{
				newJointValue[i] = motionSegment.StartJoints[i] + _currentSegmentPos*(motionSegment.TargetJoints[i]-motionSegment.StartJoints[i])/motionSegment.ChangeLength;
				newJointVel[i] = _currentSegmentVel * (motionSegment.TargetJoints[i]-motionSegment.StartJoints[i])/motionSegment.ChangeLength ;
				newJointAcc[i] = _currentSegmentAcc * (motionSegment.TargetJoints[i]-motionSegment.StartJoints[i])/motionSegment.ChangeLength ;
			}
			_pRobot->Set_JointValues(newJointValue);
			_pRobot->Set_JointVels(newJointVel);
			_pRobot->Set_JointAccs(newJointAcc);

			//if moved to end of this segment, remove it
			if(Equal(motionSegment.ChangeLength, _currentSegmentPos) == true)
			{
				_currentSegmentPos = 0;
				_motionSegments.erase(_motionSegments.begin());
				if(_motionSegments.size() <= 0)
					return 0;
			}
		}
		else if( motionSegment.MotionSegmentType == MotionSegment_Kine_CartLinear)
		{
			//get new pose, vel, acc along segment trajectory
			double newSegmentAcc = Cmd_Acc(_currentSegmentPos, _currentSegmentVel, motionSegment.ChangeLength, (_motionSegments.size()==1)?0:motionSegment.Speed);
			if(newSegmentAcc > 0)
			{
				//don't accelerate when is on Speed limit.
				if(_currentSegmentVel >= motionSegment.Speed)
					newSegmentAcc = 0;
				else
				{
					//don't make next speed out of speed limit
					double maxAcc = (motionSegment.Speed - _currentSegmentVel)/_stepInterval;
					if(newSegmentAcc > maxAcc)
						newSegmentAcc = maxAcc;
				}
			}
			double newSegmentVel = _currentSegmentVel + newSegmentAcc * _stepInterval;
			double newSegmentPos = _currentSegmentPos + _currentSegmentVel * _stepInterval + 0.5 * newSegmentAcc * _stepInterval * _stepInterval;
			if(newSegmentPos > motionSegment.ChangeLength)
			{
				newSegmentPos = motionSegment.ChangeLength;
				newSegmentVel = (newSegmentPos - _currentSegmentPos)*2/_stepInterval - _currentSegmentVel;
				newSegmentAcc = (newSegmentVel - _currentSegmentVel)/_stepInterval;
			}
			_currentSegmentPos = newSegmentPos;
			_currentSegmentVel = newSegmentVel;
			_currentSegmentAcc = newSegmentAcc;

			//get tool frame's pose, velocity and acceleration w.r.t global frame
			Vector3 moveDirection = motionSegment.TargetFrame.Translation-motionSegment.StartFrame.Translation;
			moveDirection.Normalize();
			Frame3 newToolPose = Frame3::Identity();
			newToolPose.Translation = motionSegment.StartFrame.Translation+(motionSegment.TargetFrame.Translation-motionSegment.StartFrame.Translation)*_currentSegmentPos/motionSegment.ChangeLength;
			newToolPose.Rotation = motionSegment.StartFrame.Rotation * motionSegment.ChangeOrient.Power(_currentSegmentPos/motionSegment.ChangeLength).Rotation();
			Velocity newToolVel = Velocity::Zero();
			newToolVel.LinearVel = moveDirection * _currentSegmentVel;
			Acceleration newToolAcc = Acceleration::Zero();
			newToolAcc.LinearAcc = moveDirection * _currentSegmentAcc;

			//get robot end effector's pose, velocity and acceleration w.r.t robot base frame
			Vector3 old_new = (newToolPose*_toolFrame.Inverse()).Translation - newToolPose.Translation;
			Frame3 newEEPose = _robotBaseFrame.Inverse()*newToolPose*_toolFrame.Inverse();
			Velocity newEEVel = newToolVel.RefPoint(old_new).RefBaseFrame(_robotBaseFrame.Inverse());
			Acceleration newEEAcc = newToolAcc.RefPoint(old_new, newToolVel.AngleVel).RefBaseFrame(_robotBaseFrame.Inverse());

			//get joint value, velocity and acceleration
			ColumnVector newJointValue(_pRobot->Get_Dof());
			ColumnVector newJointVel(_pRobot->Get_Dof());
			ColumnVector newJointAcc(_pRobot->Get_Dof());

			std::vector<ColumnVector> newJointValues = std::vector<ColumnVector>();
			bool ret = _pRobot->InverseKine_Pose(newEEPose, newJointValues);
			if((ret == false) || (newJointValues.size() <= 0))
				return -2;
			newJointValue = newJointValues[0];

			ret = _pRobot->InverseKine_Vel(newJointValue, newEEVel, newJointVel);
			if(ret == false)
				return -3;

			ret = _pRobot->InverseKine_Acc(newJointValue, newJointVel, newEEAcc, newJointAcc);
			if(ret == false)
				return -3;

			//set values
			_pRobot->Set_JointValues(newJointValue);
			_pRobot->Set_JointVels(newJointVel);
			_pRobot->Set_JointAccs(newJointAcc);

			//if moved to end of this segment, remove it
			if(Equal(motionSegment.ChangeLength, _currentSegmentPos) == true)
			{
				_currentSegmentPos = 0;
				_motionSegments.erase(_motionSegments.begin());
				if(_motionSegments.size() <= 0)
					return 0;
			}
		}
		else if( motionSegment.MotionSegmentType == MotionSegment_Kine_CartCycle)
		{
			//get new pose, vel, acc along segment trajectory
			double newSegmentAcc = Cmd_Acc(_currentSegmentPos, _currentSegmentVel, motionSegment.ChangeLength, (_motionSegments.size()==1)?0:motionSegment.Speed);
			if(newSegmentAcc > 0)
			{
				//don't accelerate when is on Speed limit.
				if(_currentSegmentVel >= motionSegment.Speed)
					newSegmentAcc = 0;
				else
				{
					//don't make next speed out of speed limit
					double maxAcc = (motionSegment.Speed - _currentSegmentVel)/_stepInterval;
					if(newSegmentAcc > maxAcc)
						newSegmentAcc = maxAcc;
				}
			}
			double newSegmentVel = _currentSegmentVel + newSegmentAcc * _stepInterval;
			double newSegmentPos = _currentSegmentPos + _currentSegmentVel * _stepInterval + 0.5 * newSegmentAcc * _stepInterval * _stepInterval;
			if(newSegmentPos > motionSegment.ChangeLength)
			{
				newSegmentPos = motionSegment.ChangeLength;
				newSegmentVel = (newSegmentPos - _currentSegmentPos)*2/_stepInterval - _currentSegmentVel;
				newSegmentAcc = (newSegmentVel - _currentSegmentVel)/_stepInterval;
			}
			_currentSegmentPos = newSegmentPos;
			_currentSegmentVel = newSegmentVel;
			_currentSegmentAcc = newSegmentAcc;

			//caculate Center point
			//        M
			//        /\
			//       /  \
			//     S/    \E
			//        .Center point
			//
			Vector3 S = motionSegment.StartFrame.Translation;
			Vector3 E = motionSegment.TargetFrame.Translation;
			Vector3 centerPoint = motionSegment.CenterPoint; 
			double theta = _currentSegmentPos/(S-centerPoint).Norm();
			Vector3 rotateAxis = CrossProd((S-centerPoint), (E-centerPoint));
			rotateAxis.Normalize();
			Quaternion rotateQua = Quaternion(theta,rotateAxis);
			Vector3 newSegmentPoint = centerPoint + rotateQua.Rotation()*(S-centerPoint);

			//get tool frame's pose, velocity and acceleration w.r.t global frame
			Vector3 moveDirection = CrossProd(CrossProd((S-centerPoint), (E-centerPoint)), (newSegmentPoint-centerPoint));
			moveDirection.Normalize();
			Frame3 newToolPose = Frame3::Identity();
			newToolPose.Translation = newSegmentPoint;
			newToolPose.Rotation = motionSegment.StartFrame.Rotation * motionSegment.ChangeOrient.Power(_currentSegmentPos/motionSegment.ChangeLength).Rotation();
			Velocity newToolVel = Velocity::Zero();
			newToolVel.LinearVel = moveDirection * _currentSegmentVel;
			Acceleration newToolAcc = Acceleration::Zero();
			newToolAcc.LinearAcc = moveDirection * _currentSegmentAcc;

			//get robot end effector's pose, velocity and acceleration w.r.t robot base frame
			Vector3 old_new = (newToolPose*_toolFrame.Inverse()).Translation - newToolPose.Translation;
			Frame3 newEEPose = _robotBaseFrame.Inverse()*newToolPose*_toolFrame.Inverse();
			Velocity newEEVel = newToolVel.RefPoint(old_new).RefBaseFrame(_robotBaseFrame.Inverse());
			Acceleration newEEAcc = newToolAcc.RefPoint(old_new, newToolVel.AngleVel).RefBaseFrame(_robotBaseFrame.Inverse());

			//get joint value, velocity and acceleration
			ColumnVector newJointValue(_pRobot->Get_Dof());
			ColumnVector newJointVel(_pRobot->Get_Dof());
			ColumnVector newJointAcc(_pRobot->Get_Dof());

			std::vector<ColumnVector> newJointValues = std::vector<ColumnVector>();
			bool ret = _pRobot->InverseKine_Pose(newEEPose, newJointValues);
			if((ret == false) || (newJointValues.size() <= 0))
				return -2;
			newJointValue = newJointValues[0];

			ret = _pRobot->InverseKine_Vel(newJointValue, newEEVel, newJointVel);
			if(ret == false)
				return -3;

			ret = _pRobot->InverseKine_Acc(newJointValue, newJointVel, newEEAcc, newJointAcc);
			if(ret == false)
				return -3;

			//set values
			_pRobot->Set_JointValues(newJointValue);
			_pRobot->Set_JointVels(newJointVel);
			_pRobot->Set_JointAccs(newJointAcc);

			//if moved to end of this segment, remove it
			if(Equal(motionSegment.ChangeLength, _currentSegmentPos) == true)
			{
				_currentSegmentPos = 0;
				_motionSegments.erase(_motionSegments.begin());
				if(_motionSegments.size() <= 0)
					return 0;
			}
		}
		else
		{
			throw(new exception("Unknown Motion Segment Type."));
		}

		return 1;
	}

	double MotionPlanning_Kine::Cmd_Acc(const double currentPos, const double currentVel,const double & segmentLength, const double & segmentSpeed)
	{
		//calculate joint acceleration
		double qpp = _Kp*(segmentLength - currentPos) + _Kd*(segmentSpeed - currentVel);

		return qpp;
	}

	bool MotionPlanning_Kine::GetJointValues(Frame3 endEffecter, ColumnVector& jointValues)
	{
		//get target robot pose
		std::vector<ColumnVector> jointValueList = std::vector<ColumnVector>();
		bool ret =_pRobot->InverseKine_Pose(endEffecter, jointValueList);
		ColumnVector toJointValues;
		if((ret == true) && (jointValueList.size() > 0))
		{
			jointValues = jointValueList[0];
			return true;
		}
		else
			return false;
	}
}}